

In [1]:

    
from sympy import *
init_printing()
import matplotlib.pyplot as plt
import numpy as np
% matplotlib inline
import random

Projectile Motion, 1D



In [171]:

    
dt = 1./30
tend = 1.5
t = np.arange(0, tend, dt)
p0 = 1
v0 = 6
a0 = -9.81
y = p0 + v0 * t + 0.5 * a0 * t**2

plt.figure()
plt.plot(t, y, 'b')
plt.ylim([0,4])
plt.title('Projectile motion')
plt.show()

Discrete LTI system, 1D



In [172]:

    
A = np.array([[1,dt,0],[0,1,dt],[0,0,1]])
C = np.array([[1,0,0]])



In [173]:

    
x0 = 1
v0 = 6
a0 = -9.81
z0 = np.array([[x0], [v0], [a0]])



In [174]:

    
z = np.zeros((t.size, z0.size, 1))
z[0] = z0



In [175]:

    
for i in range(t.size - 1):
    z[i+1] = A.dot(z[i])



In [176]:

    
plt.figure()
plt.plot(t, z[:,0], 'b^')
plt.ylim([0,4])
plt.title('Projectile motion, Discrete LTI System')
plt.show()

Observability



In [177]:

    
ctrl = np.array([C[0], C.dot(A)[0], C.dot(A).dot(A)[0]])



In [178]:

    
np.linalg.matrix_rank(ctrl)









    Out[178]:





$$3$$

We see that the rank of the observability matrix is 3, which == N. Our system is fully observable.

State Estimation

Assume normally distributed process noise w and measurement noise v, with normal probability distributions of mean zero and covariance Q and R.



In [201]:

    
merror = 0.1

zm = np.zeros((t.size, m, 1))
for i in range(t.size):
    zm[i] = z[i][0] + random.gauss(0, merror)



In [211]:

    
n = 3
m = 1
P = np.array([[0.1, 0.1, 0.1], [0.1, 10000, 10], [0.1, 10, 100]])
Q = np.array([[.05, 0.05, .05], [0.05, 0.05, 0.05], [.05, .05, .05]])
R = np.array([[5]])
I = np.eye(n)

zhat = np.zeros((t.size, z0.size, 1))

zhat[0] = np.array([[zm[0]], [0], [-9.81]])
for i in range(t.size - 1):
    zhat[i+1] = A.dot(zhat[i])
    P = A.dot(P).dot(np.transpose(A)) + Q
    inner = C.dot(P).dot(np.transpose(C)) + R
    K = P.dot(np.transpose(C)).dot(np.linalg.inv(inner))
    zhat[i+1] += + K.dot(zm[i+1] - C.dot(zhat[i+1]))
    P = (I - K.dot(C)).dot(P)

plt.figure()
plt.plot(t, z[:,0], 'b', label='Actual')
plt.plot(t, zhat[:,0], 'r', linewidth=3, label='Estimated')
plt.plot(t, zm[:,0], 'r^', label='Measured')
plt.title('State Estimation - Position')
plt.ylim([0,4])
plt.legend()
plt.show()

plt.figure()
plt.plot(t, z[:,1], 'b', label='Actual')
plt.plot(t, zhat[:,1], 'r', linewidth=3, label='Estimated')
plt.title('State Estimation - Velocity')
plt.ylim([-6, 6])
plt.legend()
plt.show()

plt.figure()
plt.plot(t, z[:,2], 'b', label='Actual')
plt.plot(t, zhat[:,2], 'r', linewidth=3, label='Estimated')
plt.title('State Estimation - Acceleration')
plt.ylim([-12, -8])
plt.legend()
plt.show()



In [195]:



In [ ]: